#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <math.h>
#include <iostream>
using namespace cv;

int main()
{
    VideoCapture cap(0);
    
    if(!cap.isOpened()) return -1;
    if(cap.set(CV_CAP_PROP_FRAME_WIDTH, 640)) return -1;
    if(cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480)) return -1;

    Mat img;
    Mat edges;

    namedWindow("couleur",1);
    
    vector<Vec4i> lines;
    int perspective_left_x = 0;
    int perspective_left_y = 0;
    int perspective_right_x = 0;
    int perspective_right_y = 0;
    for (;;)
    {
        // Capture an image frome camera
        cap >> img;

        // Pre filter the image (extract the black and fine lines in it)
        cvtColor(img, edges, CV_BGR2GRAY);
        GaussianBlur(edges, edges, Size(7,5), 640, 480);
        threshold(edges, edges, 50, 255, THRESH_BINARY);
        Canny(edges, edges, 1, 15);
        HoughLinesP(edges, lines, 1, CV_PI/180, 10, 30, 2);
       
        int x1, y1, x2, y2, candidate_left_x, candidate_left_y, 
            candidate_right_x, candidate_right_y;
        for( size_t i = 0; i < lines.size(); i++ )
        {
            x1 = lines[i][0];
            y1 = lines[i][1];
            x2 = lines[i][2];
            y2 = lines[i][3];

            // Isolate lines
            int xdiff = abs(x2 - x1);
            int ydiff = abs(y2 - y1);
            
            if (xdiff > 10 && ydiff > 10)
            {
                // Debug
                line( img, Point(lines[i][0], lines[i][1]),
                Point(lines[i][2], lines[i][3]), Scalar(255,0,255), 3, 8 );
                
                candidate_left_x = max(x1,x2);
                candidate_left_y = min(y1,y2);
                candidate_right_x = min(x1,x2);
                candidate_right_y = max(y1,y2);

                // Easy way to determine if we are on the left or not
                if (x1 < 640 / 2)
                {
                    if (perspective_left_x == 0 ||
                        (perspective_left_x < candidate_left_x &&
                        abs(perspective_left_x - candidate_left_x) < 10))
                    {
                        perspective_left_x = candidate_left_x;
                    }

                    if (perspective_left_y == 0 ||
                        (perspective_left_y > candidate_left_y &&
                        abs(perspective_left_y - candidate_left_y) < 10))
                    {
                        perspective_left_y = candidate_left_y;
                    }
                    
                }
                else
                {
                    if (perspective_right_x == 0 ||
                        (perspective_right_x > candidate_right_x &&
                        abs(perspective_right_x - candidate_right_x) < 10)
                        )
                    {
                        perspective_right_x = candidate_right_x;
                    }

                    if (perspective_right_y == 0 ||
                        (perspective_right_y > candidate_right_y &&
                        abs(perspective_right_y - candidate_right_y) < 10))
                    {
                        perspective_right_y = candidate_right_y;
                    }
                }
            }
        }
        // Show the perspective points on the image        
        circle(img, Point(perspective_left_x, perspective_left_y), 
            2, Scalar(255, 0, 0), 2);
        circle(img, Point(perspective_right_x, perspective_right_y), 
            2, Scalar(255, 0, 0), 2);

        imshow("couleur", img);
        waitKey(30);

   } 
   return 0;
}

